#ifndef SWARM_AVOID_HPP
#define SWARM_AVOID_HPP

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/bool.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "sensor_msgs/msg/nav_sat_fix.hpp"
// #include "px4_msgs/msg/vehicle_global_position.hpp"
// #include <tf2_ros/transform_listener.h>
// #include <tf2/transform_datatypes.h>
#include "mavros_msgs/msg/state.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "mavros_msgs/srv/set_mode.hpp"
#include "mavros_msgs/srv/command_bool.hpp"
#include "mavros_msgs/msg/position_target.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "geometry_msgs/msg/vector3_stamped.hpp"
#include <asio.hpp>
#include <memory>
#include <string>
#include "swarm_computer.hpp"
#include "interfaces/srv/set_goal.hpp"
#include "yolo_detect_message/msg/move_interface.hpp"
// #include <

using Eigen::Vector3d;


// 定义一个结构体来存储解码后的数据
struct udpData {
    double lat;  // 纬度
    double lon;  // 经度
    double alt;  // 高度
    double radius; // 障碍物半径
    std::string name;  // 类别
    int id; // 物体唯一标识
};


class AirplaneController : public rclcpp::Node {
public:
    AirplaneController(const std::string& namespace_prefix);
    ~AirplaneController() = default;

    bool add_obstacle(const Obstacle& obstacle, int index);

private:

    int id_;

    // Group multicast receiver variables
    std::shared_ptr<asio::ip::udp::socket> multicast_socket_;
    std::shared_ptr<asio::io_context> io_context_;
    asio::ip::udp::endpoint multicast_endpoint_;
    std::string multicast_address_;
    uint16_t multicast_port_;

    void startMulticastReceiver(const std::string& ip, int port);
    void handleMulticastMessage(const udpData data, Obstacle& obstacle, int &index) ;
    udpData decode_location(const std::string& json_data);

    // Class members
    std::string namespace_prefix_;

    swarm_controller controller_;  // Swarm control
   
    bool takeoff_signal_received_;
    sensor_msgs::msg::NavSatFix initial_position_; // Record takeoff position
    bool initial_position_recorded_;
    bool flag_init_position;
    bool init_flag ;
    bool flag_arrive ;
    bool first_detected ;
    bool takeoff_in_progress_ ;  // Track takeoff state
    double target_takeoff_height_ ;  // Takeoff target height

    mavros_msgs::msg::State current_state_;  // Current state of the drone
    geometry_msgs::msg::PoseStamped local_pose_;  // Local position from MAVROS
    geometry_msgs::msg::Twist current_velocity_;  // Current velocity
    rclcpp::Time last_request_;
    yolo_detect_message::msg::MoveInterface target_motion_;
    
    int wait_times;
    // 无人机的当前运动类型
    int motion_type_;

    // 坐标系标志（例如 1 表示 ENU 坐标系）
    std::mutex coordinate_frame_mutex_;
    int coordinate_frame_ = 1;

    mavros_msgs::msg::PositionTarget construct_target(
    double x, double y, double z, double vx, double vy, double vz,
    double afx, double afy, double afz, double yaw, double yaw_rate);

    // Goal variables
    double goal_x_, goal_y_, goal_z_;  // Goal coordinates
    bool goal_received_ ;  // Flag to indicate if the goal was received

    std::string namespaces;
    int vehcile_id_;
    int vehcile_num_ ;

    // Position variables for takeoff
    double init_position_x_take_off;
    double init_position_y_take_off;
    double init_position_z_take_off;

    // Mutexes
    std::mutex current_velocity_mutex_;
    std::mutex wait_times_mutex_;

    // ROS 2 components

    rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr state_sub_;
    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr local_pos_sb;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr mavros_velocity_pub_;
    rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr mavros_position_pub_;

    rclcpp::Publisher<yolo_detect_message::msg::MoveInterface>::SharedPtr target_motion_pub_;
    rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr set_mode_client_;
    rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr arming_client_;
    rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr takeoff_client_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Service<interfaces::srv::SetGoal>::SharedPtr set_goal_service_;
    std::vector<rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr> other_airplane_pos_subscribers_;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr takeoff_sync_sub_;

    // Store other drones' positions
    std::unordered_map<std::string, sensor_msgs::msg::NavSatFix> other_airplane_positions_;
    // std::unordered_map<std::string, geometry_msgs::msg::TransformStamped> other_airplane_transforms_;

    // std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
    // std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

    // Global position (using Eigen's Vector3d)
    Vector3d current_position_;  // Current drone position in global coordinates
    std::unordered_map<std::string, Vector3d> other_positions_;  // Other drones' positions in global coordinates

    // Initialization functions
    void initializeSubscribers(const std::vector<std::string>& other_airplanes);
    void initializePublishers();
    void initializeServiceClients();
    void initializeServiceServer();
    void initializeTimer();

    // Callback functions
    void takeoffSyncCallback(const std_msgs::msg::Bool::SharedPtr msg);
    void stateCallback(const mavros_msgs::msg::State::SharedPtr msg);
    void localPosCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg);
    void otherAirplanePosCallback(const std::string& airplane_id, const sensor_msgs::msg::NavSatFix::SharedPtr msg);
    void setGoalCallback(const std::shared_ptr<rmw_request_id_t> request_header, 
                        const std::shared_ptr<interfaces::srv::SetGoal::Request> request, 
                        const std::shared_ptr<interfaces::srv::SetGoal::Response> response);

    // Control functions
    // // 初始化无人机的离线模式和解锁状态,设置无人机的offboard模式和解锁状态
    // void init_offboard_and_arm();

    // void set_offboard_mode();
    // void arm_drone();

    void armDrone();
    void setOffboardMode();
    void setVelocity(const Vector3d& velocity);
    void resetVelocity();
    void setVelocityZ(float z);

    // Timer loop callback
    void timerLoop();
};

#endif // SWARM_AVOID_HPP
